//
// Created by chen on 25-5-10.
//

#include "CurrentControlLoop.h"
#include <cmath>

CurrentControlLoop::CurrentControlLoop(float kp, float ki, float integralLimit, float outputLimit, float dcBusVoltage)
    : ControlLoop(kp, ki, integralLimit, outputLimit, dcBusVoltage),
      dAxisController(kp, ki, integralLimit, outputLimit),
      qAxisController(kp, ki, integralLimit, outputLimit) {}

/**
 * 这个函数是磁场定向控制（FOC）中电流环的核心部分，
 * 主要作用是根据电流指令和电机转子位置，
 * 计算出静止坐标系（αβ 轴）下的电压矢量，
 * 为后续的空间矢量脉宽调制（SVPWM）提供输入
 * @param angle
 * @param targetId
 * @param targetIq
 */
void CurrentControlLoop::execute(float angle, float targetId, float targetIq) {
    // 分别对d轴和q轴电流进行PI控制
    float outputD = dAxisController.compute(targetId, actualValue);
    float outputQ = qAxisController.compute(targetIq, actualValue);

    // 保存输出
    this->outputD = outputD;
    this->outputQ = outputQ;

    // 执行反Park变换
    float cosTheta = std::cos(angle);
    float sinTheta = std::sin(angle);
    valpha = outputD * cosTheta - outputQ * sinTheta;
    vbeta = outputD * sinTheta + outputQ * cosTheta;
}

void CurrentControlLoop::execute(float angle) {
    execute(angle, 0.0f, targetValue);
}